#ifndef CVX_K_F_CALIB_H
#define CVX_K_F_CALIB_H 1

//key frame from video to automatic calibration
#include "cvxImage.h"
#include <string>
#include "cvxBoard.h"
#include "CCamera.h"
#include "cvxAnn.h"

using std::string;

struct KFCalibSelectCriteria
{
	int  _cameraFlags;                   //like CV_CALIB_FIX_K2
	bool _writeFile;                     //write intermediate result to files
	bool _printOutMsg;                   //print out message for each step
	bool _readPtsFromFile;               //read corner position from (.txt) file
	bool _isTracking;                    //tracking when detect corner position
	bool _checkCornerPos;                //check corner position 
	bool _showFrame;
	CvCapture *_pCap;                    // capture of video file
	string _saveFolder;                  // folder to save intermediate files
	float _clearThd;					 //threshold for no blur;
	float _difPoseThd;					 //threshold for different pose in clear image sequence

	float _kdDisThd;                     //kd-tree distance threshold
	

	KFCalibSelectCriteria()
	{
		_cameraFlags = CV_CALIB_FIX_K2|CV_CALIB_FIX_K3|CV_CALIB_FIX_K4|CV_CALIB_FIX_PRINCIPAL_POINT;
		_writeFile = true;		
		_printOutMsg = true;
		_readPtsFromFile = true;
		_isTracking = false;
		_checkCornerPos = false;
		_showFrame = false;
		_pCap = NULL;
		_clearThd = 0.5;
		_difPoseThd = 40;
		_kdDisThd = 0.08f;		
	}
	
};

class CvxKFCalib
{
public:
	CvxKFCalib();
	~CvxKFCalib();

public:

	/*
	input:  vf: video file name
			ptf: point position file name
			one of videofileName and ptsFileName should be empty
			target: a plane board
			criteria: calibration configuration for camera model and threshold
	output: kFI: key frame index in the original video file or point position file
	*/
	bool select(const string &vf, const string &ptf, 
			    CvxBoard *ptarget, const KFCalibSelectCriteria &criteria,  vector<int> &kFI);	

private:
	vector<vector<Point2f>> m_pts; // points in blur-free frames
	vector<int>   m_idxes;           //clear frame index of original frame 
	vector<float> m_offsets;        //average offset between previous and next frames
	KFCalibSelectCriteria *m_pCriteria;  //criteria of automatic selection
	CCamera               *m_pCamera;    //camera intrinsic parameters
	CvxAnn				  *m_pkdTree;    //kd-tree to store normal of target plane
	vector<int>   m_optimalIdxes;        //optimal index of clear points 

public:

	/************************************************************************/
	/* 
	vf:      video file name
	savef:   save file name. (.txt)
	isTracking: tracking too speed up
	ptarget: a plane board
	imageSize: return the image resolution
	*/
	/************************************************************************/
	static bool SaveCorners(const string &vf, const string &savef, const CvxBoard *ptarget,
						    bool isTracking, CvSize &imageSize);
	/************************************************************************/
	/* 
	load corner position, index and offset with previous and next frame from a (.txt) file

	*/
	/************************************************************************/
	static bool LoadCorners(const string &fileName, vector<int> &idxVec, 
						    vector<vector<Point2f>> &ptsVec, vector<float> &offset,
							CvSize &patternSize, CvSize &frameSize);

private:
	// computer none blur frame from video or chessboard position file(.txt)
	bool GetNoblurFrames(const string &vf, const string &ptf, CvxBoard *ptarget, CvSize &imageSize);
	// idx is of the m_idxes
	void GetInitialFrameIndex(vector<int> &idxVec);

	//get camera parameter from initial frames
	void initCamera(const vector<int> &idxVec, const CvSize &imageSize, const CvxBoard *ptarget);

	//init kd tree from initual index
	void initKdTree(const vector<int> &idxVec, const CvxBoard *ptarget);

	//add frame to kd-tree based on minimal distance to kdtree
	bool AddFrames(const CvxBoard *ptarget, const vector<int> &curIdxVec);

	//delete frame in the kd-tree
	bool DeleteFrames(const CvxBoard *ptarget, const vector<int> &curIdxVec);

	//update kd-tree based on optimized index
	void updateKdTree(const vector<int> &idxVec, const CvxBoard *ptarget);

	//update camera parameters
	void updateCamera(const CvSize &imageSize, const CvxBoard *ptarget);



	//test
	static bool SaveIntermResult(const string &vf, const string &savef, vector<int> &idxVec, 
								 vector<vector<Point2f>> &ptsVec, vector<float> &offsetVec, 
								 const CvSize &patternSize, const CvSize &frameSize);
	//test
	static bool SavePose(const string &savef, const vector<Point3f> &poseVec);

	//test 
	static bool SaveIdx(const string &savef, const vector<vector<int>> &idxVec);

	//angle difference in [0, 360) range
	static float angleDifference(float angleA, float angleB);

	

};



#endif